package oneD.dynamics;

import inf.jlinalg.Array2DMatrix;
import inf.jlinalg.ArrayVector;
import inf.jlinalg.BLAM;
import inf.jlinalg.GeneralMatrixLSESolver;
import inf.jlinalg.ILSESolver;
import inf.jlinalg.IMatrix;
import inf.jlinalg.IVector;
import inf.jlinalg.MatrixFormat;
import inf.jlinalg.QuadraticMatrixInfo;
import inf.jlinalg.SolveFailedException;
import oneD.fem.algorithm.MyLinalg;

public class LinearTimeIntegrator {
	/*
	 * Class to compute dynamic response of linear equation M udd + D ud + K u =
	 * r The damping model used is Rayleigh damping model D = alpha1*M +
	 * alpha2*K. By default, alpha1 = 0 and alpha2 = 0 means no damping
	 */
	private DynamicStructure struct_;
	private double alpha1_;
	private double alpha2_;
	private double dt_;
	private int nStep_;
	private boolean AccelerationComplementaryConditionAtInitial;

	public LinearTimeIntegrator(DynamicStructure struct) {
		this.struct_ = struct;
		this.setAccelerationComplementaryConditionAtInitial(false);
	}

	public void setAlpha1(double alpha1_) {
		this.alpha1_ = alpha1_;
	}

	public void setAlpha2(double alpha2_) {
		this.alpha2_ = alpha2_;
	}

	public void setTimeStep(double dt_) {
		this.dt_ = dt_;
		this.struct_.setTimeStepForPlot(dt_);
	}

	public double getTimeStep() {
		return this.dt_;
	}

	public void Initialize(int numIterationStep) {
		/*
		 * Initialize the number of iteration
		 */
		this.nStep_ = numIterationStep;
		this.struct_.Initialize();
	}

	public void freeVibCDM() {
		ILSESolver solver = new GeneralMatrixLSESolver(
				GeneralMatrixLSESolver.SOLVER_IMPL_JAVA);
		// using this for solver implementation in Java, it will be slow
		QuadraticMatrixInfo aInfo = solver.getAInfo();
		int NEQ = this.struct_.getStaticStructure().getNumberOfEquations();
		IMatrix Keff = solver.getA();

		aInfo.setSize(NEQ);
		solver.initialize();

		this.struct_.assembleKeffForCDM(Keff, this.alpha1_, this.alpha2_,
				this.dt_);

		System.out.println("Efficient stiffness matrix");
		System.out.println(MatrixFormat.format(Keff));

		IMatrix K = (IMatrix) Keff.clone();
		IMatrix M = (IMatrix) Keff.clone();
		BLAM.zero(K);
		BLAM.zero(M);
		this.struct_.getStaticStructure().assembleStiffnessMatrix(K, true);
		this.struct_.getStaticStructure().assembleMassMatrix(M);

		System.out.println("Stiffness matrix");
		System.out.println(MatrixFormat.format(K));
		System.out.println("Mass matrix");
		System.out.println(MatrixFormat.format(M));

		IMatrix D = (IMatrix) M.clone();
		MyLinalg.scale(alpha1_, D);
		BLAM.add(alpha2_, K, D);

		System.out.println("Damping matrix");
		System.out.println(MatrixFormat.format(D));

		IVector Un = this.struct_.getDisplacementVector(0);
		IVector Und = this.struct_.getVelocityVector(0);
		IVector Undd = (IVector) Un.clone();
		BLAM.fill(Undd, 0);
		if (!this.AccelerationComplementaryConditionAtInitial)
			Undd = this.struct_.getAccelerationVector(0);
		else {
			IVector temp = (IVector) Undd.clone();
			MyLinalg.multiply(1, BLAM.NO_TRANSPOSE, K, Un, 0, temp);
			MyLinalg.multiply(-1, BLAM.NO_TRANSPOSE,
					MyLinalg.inverseDiagonalMatrix(M), temp, 0, Undd);
			this.struct_.addAcceleration(Undd);
		}

		IVector Unm1 = (IVector) Un.clone();
		MyLinalg.add(-dt_, Und, Unm1);
		MyLinalg.add(dt_ * dt_ / 2, Undd, Unm1);

		IVector reff = (IVector) Un.clone();
		BLAM.fill(reff, 0);

		for (int i = 0; i < this.nStep_; i++) {
			BLAM.fill(reff, 0);
			IVector temp = (IVector) Un.clone();
			MyLinalg.multiply(-1, BLAM.NO_TRANSPOSE, K, Un, 0, reff);
			MyLinalg.multiply(0.5 / dt_, BLAM.NO_TRANSPOSE, D, Unm1, 1, reff);
			MyLinalg.add(-0.5, Unm1, temp);
			MyLinalg.multiply(2 / (dt_ * dt_), BLAM.NO_TRANSPOSE, M, temp, 1,
					reff);

			// System.out.println("Reff " + (i + 1) + "="
			// + MatrixFormat.format(reff));

			double[] b = MyLinalg.toArray(reff);

			try {
				solver.solve(b);
			} catch (SolveFailedException e) {
				// TODO Auto-generated catch block
				e.printStackTrace();
			}

			IVector Unm2 = (IVector) Unm1.clone();
			Unm1 = (IVector) Un.clone();
			Un = new ArrayVector(b);

			this.struct_.addDisplacement(Un);
			// System.out.println("Un " + (i + 1) + "=" +
			// MatrixFormat.format(Un));

			temp = (IVector) Un.clone();
			MyLinalg.add(-1, Unm2, temp);
			MyLinalg.scale(0.5 / dt_, temp);
			this.struct_.addVelocity(temp);

			temp = (IVector) Un.clone();
			MyLinalg.add(-2, Unm1, temp);
			MyLinalg.add(1, Unm2, temp);
			MyLinalg.scale(1 / (dt_ * dt_), temp);
			this.struct_.addAcceleration(temp);
		}

	}

	public void freeVibDG1() {
		/*
		 * perform dynamic analysis using discontinuous Galerkin method with
		 * standard convention An=Au=K,Av=M
		 */
		ILSESolver solver = new GeneralMatrixLSESolver(
				GeneralMatrixLSESolver.SOLVER_IMPL_JAVA);
		// using this for solver implementation in Java, it will be slow
		QuadraticMatrixInfo aInfo = solver.getAInfo();
		int NEQ = this.struct_.getStaticStructure().getNumberOfEquations();
		IMatrix Kdg1 = solver.getA();
		IMatrix K = new Array2DMatrix(NEQ, NEQ);
		IMatrix M = (IMatrix) K.clone();

		aInfo.setSize(4 * NEQ);
		solver.initialize();

		this.struct_.assembleKdg1(Kdg1, K, M, this.alpha1_, this.alpha2_,
				this.dt_);
		System.out.println("Kdg1 = " + MatrixFormat.format(Kdg1));

		IVector Un = this.struct_.getDisplacementVector(0);
		IVector Und = this.struct_.getVelocityVector(0);
		IVector Undd = (IVector) Un.clone();
		BLAM.fill(Undd, 0);
		if (!this.AccelerationComplementaryConditionAtInitial)
			Undd = this.struct_.getAccelerationVector(0);
		else {
			IVector temp = (IVector) Undd.clone();
			MyLinalg.multiply(1, BLAM.NO_TRANSPOSE, K, Un, 0, temp);
			MyLinalg.multiply(-1, BLAM.NO_TRANSPOSE,
					MyLinalg.inverseDiagonalMatrix(M), temp, 0, Undd);
			this.struct_.addAcceleration(Undd);
		}

		IVector rdg = new ArrayVector(4 * NEQ);
		IMatrix Au = K;
		IMatrix Av = M;

		for (int i = 0; i < this.nStep_; i++) {
			BLAM.fill(rdg, 0);
			IVector temp1 = (IVector) Un.clone();
			IVector temp2 = (IVector) Un.clone();
			MyLinalg.multiply(1, BLAM.NO_TRANSPOSE, Au, Un, 0, temp1);
			MyLinalg.multiply(1, BLAM.NO_TRANSPOSE, Av, Und, 0, temp2);
			for (int j = 0; j < NEQ; j++) {
				rdg.set(j, temp1.get(j));
				rdg.set(j + 2 * NEQ, temp2.get(j));
			}

//			System.out.println("rdg" + i + "=" + MatrixFormat.format(rdg));

			double[] b = MyLinalg.toArray(rdg);

			try {
				solver.solve(b);
			} catch (SolveFailedException e) {
				System.out.println(e.toString());
			}
			MyLinalg.printVectorDouble(b);

			for (int j = 0; j < NEQ; j++) {
				Un.set(j, b[j]);
				Und.set(j, b[j + 2 * NEQ]);
			}
			this.struct_.addDisplacement(Un);
			this.struct_.addVelocity(Und);

			for (int j = 0; j < NEQ; j++) {
				Un.set(j, b[j + NEQ]);
				Und.set(j, b[j + 3 * NEQ]);
			}
		}
	}

	public void freeVibDG2() {
		/*
		 * perform dynamic analysis using discontinuous Galerkin method with
		 * standard convention An=Au=K,Av=M
		 */
		ILSESolver solver = new GeneralMatrixLSESolver(
				GeneralMatrixLSESolver.SOLVER_IMPL_JAVA);
		// using this for solver implementation in Java, it will be slow
		QuadraticMatrixInfo aInfo = solver.getAInfo();
		int NEQ = this.struct_.getStaticStructure().getNumberOfEquations();
		IMatrix Kdg1 = solver.getA();
		IMatrix K = new Array2DMatrix(NEQ, NEQ);
		IMatrix M = (IMatrix) K.clone();

		aInfo.setSize(6 * NEQ);
		solver.initialize();

		this.struct_.assembleKdg2(Kdg1, K, M, this.alpha1_, this.alpha2_,
				this.dt_);
		System.out.println("Kdg2 = " + MatrixFormat.format(Kdg1));

		IVector Un = this.struct_.getDisplacementVector(0);
		IVector Und = this.struct_.getVelocityVector(0);
		IVector Undd = (IVector) Un.clone();
		BLAM.fill(Undd, 0);
		if (!this.AccelerationComplementaryConditionAtInitial)
			Undd = this.struct_.getAccelerationVector(0);
		else {
			IVector temp = (IVector) Undd.clone();
			MyLinalg.multiply(1, BLAM.NO_TRANSPOSE, K, Un, 0, temp);
			MyLinalg.multiply(-1, BLAM.NO_TRANSPOSE,
					MyLinalg.inverseDiagonalMatrix(M), temp, 0, Undd);
			this.struct_.addAcceleration(Undd);
		}

		IVector rdg = new ArrayVector(6 * NEQ);
		IMatrix Au = K;
		IMatrix Av = M;

		for (int i = 0; i < this.nStep_; i++) {
			BLAM.fill(rdg, 0);
			IVector temp1 = (IVector) Un.clone();
			IVector temp2 = (IVector) Un.clone();
			MyLinalg.multiply(1, BLAM.NO_TRANSPOSE, Au, Un, 0, temp1);
			MyLinalg.multiply(1, BLAM.NO_TRANSPOSE, Av, Und, 0, temp2);
			for (int j = 0; j < NEQ; j++) {
				rdg.set(j, temp1.get(j));
				rdg.set(j + 3 * NEQ, temp2.get(j));
			}

			// System.out.println("rdg" + i + "=" + MatrixFormat.format(rdg));

			double[] b = MyLinalg.toArray(rdg);

			try {
				solver.solve(b);
			} catch (SolveFailedException e) {
				System.out.println(e.toString());
			}

			for (int j = 0; j < NEQ; j++) {
				Un.set(j, b[j]);
				Und.set(j, b[j + 3 * NEQ]);
			}
			this.struct_.addDisplacement(Un);
			this.struct_.addVelocity(Und);

			for (int j = 0; j < NEQ; j++) {
				Un.set(j, b[j + 2 * NEQ]);
				Und.set(j, b[j + 5 * NEQ]);
			}
		}
	}

	public void freeVibDG(int p) {
		/*
		 * perform dynamic analysis using discontinuous Galerkin method with
		 * standard convention An=Au=K,Av=M
		 */
		ILSESolver solver = new GeneralMatrixLSESolver(
				GeneralMatrixLSESolver.SOLVER_IMPL_JAVA);
		// using this for solver implementation in Java, it will be slow
		QuadraticMatrixInfo aInfo = solver.getAInfo();
		int NEQ = this.struct_.getStaticStructure().getNumberOfEquations();
		IMatrix Kdg = solver.getA();
		IMatrix K = new Array2DMatrix(NEQ, NEQ);
		IMatrix M = (IMatrix) K.clone();

		aInfo.setSize(2 * (p + 1) * NEQ);
		solver.initialize();

		this.struct_.assembleKdgp(p, Kdg, K, M, this.alpha1_, this.alpha2_,
				this.dt_);
		System.out.println("Kdg" + p + " = " + MatrixFormat.format(Kdg));

		IVector Un = this.struct_.getDisplacementVector(0);
		IVector Und = this.struct_.getVelocityVector(0);
		IVector Undd = (IVector) Un.clone();
		BLAM.fill(Undd, 0);
		if (!this.AccelerationComplementaryConditionAtInitial)
			Undd = this.struct_.getAccelerationVector(0);
		else {
			IVector temp = (IVector) Undd.clone();
			MyLinalg.multiply(1, BLAM.NO_TRANSPOSE, K, Un, 0, temp);
			MyLinalg.multiply(-1, BLAM.NO_TRANSPOSE,
					MyLinalg.inverseDiagonalMatrix(M), temp, 0, Undd);
			this.struct_.addAcceleration(Undd);
		}

		IVector rdg = new ArrayVector(2 * (p + 1) * NEQ);
		IMatrix Au = K;
		IMatrix Av = M;

		for (int i = 0; i < this.nStep_; i++) {
			BLAM.fill(rdg, 0);
			IVector temp1 = (IVector) Un.clone();
			IVector temp2 = (IVector) Un.clone();
			MyLinalg.multiply(1, BLAM.NO_TRANSPOSE, Au, Un, 0, temp1);
			MyLinalg.multiply(1, BLAM.NO_TRANSPOSE, Av, Und, 0, temp2);
			for (int j = 0; j < NEQ; j++) {
				rdg.set(j, temp1.get(j));
				rdg.set(j + (p + 1) * NEQ, temp2.get(j));
			}

//			System.out.println("rdg" + i + "=" + MatrixFormat.format(rdg));

			double[] b = MyLinalg.toArray(rdg);

			try {
				solver.solve(b);
			} catch (SolveFailedException e) {
				System.out.println(e.toString());
			}
//			BLAMpatch.printVectorDouble(b);
			
			for (int j = 0; j < NEQ; j++) {
				Un.set(j, b[j]);
				Und.set(j, b[j + (p + 1) * NEQ]);
			}
			this.struct_.addDisplacement(Un);
			this.struct_.addVelocity(Und);
			this.struct_.addAcceleration(Undd);
			
			for (int j = 0; j < NEQ; j++) {
				Un.set(j, b[j + p * NEQ]);
				Und.set(j, b[j + (2 * p + 1) * NEQ]);
			}
		}
	}

	public void setAccelerationComplementaryConditionAtInitial(
			boolean accelerationComplementaryConditionAtInitial) {
		AccelerationComplementaryConditionAtInitial = accelerationComplementaryConditionAtInitial;
	}

}
